//
// Created by yu on 2023/9/24.
//
#include "CANFrame.h"
#include <yaml-cpp/yaml.h>

class Motor
{
private:
    class legPID
    {
    public:
        double integral;
        double derivative;
        double prev_error;
        double kP;
        double kI;
        double kD;
    } leftLegPID, rightLegPID;

    class balancePID
    {
    public:
        double integral;
        double derivative;
        double prev_error;
        double kP;
        double kI;
        double kD;
    } balancePID;

    class speedPID
    {
    public:
        double integral;
        double derivative;
        double prev_error;
        double kP;
        double kI;
        double kD;
    } speedPID;

public:
    uint16_t leftZero;
    uint16_t rightZero;
    int lefttimesCount;
    int righttimesCount;
    double leftLastAngle;
    double rightLastAngle;

/// 读取配置文件
    void readYaml();

/// usb->can模块指令模式启动
    void controlBegin();

/// 全部停止
    void controlStop();

/// 平衡环pid
    void balancePidControl();

/// 速度环pid
    double velocityPidControl();

/// 腿高pid
    void legHeightControl(int id, int angle);

/// 腿高平衡
    void rollBalance();

/// pid 积分误差清零
    void pidReset();

/// 腿复位
    void legReset();

/// 恢复平衡
    void standUp();

} inline Motor;

inline YAML::Node config;